﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using PloobsEngine.Utils;
using PloobsEngine.Physics.XJigLib;

namespace PloobsEngine.IA.Locomotion
{
    public class BasicSteering
    {
        public BasicSteering(CharacterObject obj)
        {
            Owner = obj;
            AngleError = 2f;
            VelocityError = 1;
            ArriveDistance = 20;
            BrakeDistanceError = 6;
            Ended = true;
        }

          
        private float _speed = 0;

        /// <summary>
        /// Gets or sets the maximum speed the agent can travel at
        /// </summary>
        public float MaxSpeed
        {
            get;
            set;
        }

        public float MaxAceleration
        {
            get;
            set;
        }

        Vector3 _dpos;
        public Vector3 DesiredPosition
        {
            set
            {
                _dpos = value;
                Ended = false;
            }
            get
            {
                return _dpos;
            }
        }


        /// <summary>
        /// Gets or sets the maximum amount an Agent can rotate per second
        /// </summary>
        /// <remarks>Value is in degrees</remarks>
        public float MaxRotation
        {
            get;
            set;
        }

        public CharacterObject Owner
        {
            get;
            set;
        }

        public float AngleError
        {
            get;
            set;
        }

        public float VelocityError
        {
            get;
            set;
        }

        public float ArriveDistance
        {
            get;
            set;
        }

        public float BrakeDistanceError
        {
            set;
            get;
        }

        public bool Ended
        {
            set;
            get;
        }        

        /// <summary>
        /// Gets the current speed of the Agent
        /// </summary>
        public float DesiredSpeed
        {
            set
            {
                this._speed = value;
                Ended = false;
            }
            get { return _speed; }
        }

        private Vector3 brakeInitPosition;
        bool freaiado = false;

        public void Update(GameTime gt)
        {
            if (Ended)
            {
                Owner.CharacterBody.DesiredVelocity = Vector3.Zero;
                Owner.CharacterBody.VelocityAux = Vector3.Zero;
                return;
            }

            if (Owner.isMotionLess)
                return;                        
              
            float timeFactor = (float)gt.ElapsedGameTime.TotalSeconds;

            if (timeFactor == 0.0f)
                return;

            bool arriving = false;
            Vector3 desiredOrientation = DesiredPosition - Owner.Position;
            float ang = (float)VectorUtils.FindAngleBetweenTwoVectors2D(Owner.FaceVector, desiredOrientation);

            if (Math.Abs(ang - Owner.RotationAngle) > MathHelper.ToRadians(AngleError))
            {
                float rad = MathHelper.ToRadians(MaxRotation);
                float absangle = Math.Abs(ang);
                float val = rad * timeFactor;

                if (absangle > val)
                {
                    if (ang > 0)
                        Owner.RotationAngle += MathHelper.ToDegrees(val);
                    else
                        Owner.RotationAngle -= MathHelper.ToDegrees(val);
                }
                else
                {
                    Owner.RotationAngle += MathHelper.ToDegrees(ang);
                }
            }

            if (Owner.CharacterBody.OldVelocity.Length() > Owner.CharacterBody.Velocity.Length())
            {
                ///ve se ta na distancia minima
                if (desiredOrientation.Length() < ArriveDistance)
                {
                    if (freaiado == false)
                    {
                        brakeInitPosition = Owner.Position;
                    }

                    float dist = (Owner.Position - brakeInitPosition).Length();
                    dist = 1 - (ArriveDistance - dist) / ArriveDistance;
                    dist = MathHelper.Clamp(dist, 0, 1);
                    float vel = MathHelper.Lerp(desiredOrientation.Length(), 0, dist);
                    Owner.CharacterBody.DesiredVelocity = Owner.FaceVector * vel;
                    arriving = true;
                    freaiado = true;

                    if (Owner.Velocity.Length() < 0.5f)
                    {
                        Owner.CharacterBody.DesiredVelocity = Vector3.Zero;
                        Owner.CharacterBody.VelocityAux = Vector3.Zero;
                        freaiado = false;
                        Ended = true;
                    }
                }
            }         


            if (arriving==false && Math.Abs(DesiredSpeed - Owner.Velocity.Length()) > VelocityError)
            {
                float dif = DesiredSpeed - Owner.Velocity.Length();
                if (Math.Abs(dif) > MaxAceleration * timeFactor)
                {
                    if (dif > 0)
                    {
                        Owner.CharacterBody.DesiredVelocity = Owner.FaceVector * (MaxAceleration * timeFactor + Owner.Velocity.Length());
                    }
                    else
                    {
                        Owner.CharacterBody.DesiredVelocity = Owner.FaceVector * (-MaxAceleration * timeFactor + Owner.Velocity.Length());
                    }
                    
                }
                else
                {
                    Owner.CharacterBody.DesiredVelocity = Owner.FaceVector * (DesiredSpeed + Owner.Velocity.Length());
                }
            }

        }
    }


}

